// File: src/modbus_rtu.cpp
#include "../include/sensor_cpp_pkg/modbus_rtu.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <sys/select.h>
#include <cstring>
#include <iostream>

ModbusRTU::ModbusRTU(const std::string& port, int baud_rate, int slave_address)
    : port_(port), baud_rate_(baud_rate), slave_address_(slave_address), serial_fd_(-1) {}

ModbusRTU::~ModbusRTU() {
    disconnect();
}

bool ModbusRTU::connect() {
    serial_fd_ = open(port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (serial_fd_ < 0) {
        std::cerr << "无法打开串口设备: " << port_ << std::endl;
        return false;
    }

    fcntl(serial_fd_, F_SETFL, 0);  // 恢复阻塞模式

    struct termios options;
    tcgetattr(serial_fd_, &options);

    cfsetispeed(&options, baud_rate_);
    cfsetospeed(&options, baud_rate_);

    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;         // 8位数据位
    options.c_cflag &= ~PARENB;     // 无校验
    options.c_cflag &= ~CSTOPB;     // 1位停止位
    options.c_cflag |= CLOCAL;      // 忽略调制解调器控制线
    options.c_cflag |= CREAD;       // 启用接收

    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // 非规范输入
    options.c_oflag &= ~OPOST;                         // 原始输出

    options.c_cc[VMIN] = 0;     // 最小字符数
    options.c_cc[VTIME] = 10;   // 超时时间 (单位：十分之一秒)

    if (tcsetattr(serial_fd_, TCSANOW, &options) != 0) {
        std::cerr << "配置串口失败" << std::endl;
        close(serial_fd_);
        serial_fd_ = -1;
        return false;
    }

    return true;
}

void ModbusRTU::disconnect() {
    if (serial_fd_ >= 0) {
        close(serial_fd_);
        serial_fd_ = -1;
    }
}

uint16_t ModbusRTU::calculateCRC(const uint8_t* data, size_t length) {
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < length; ++i) {
        crc ^= data[i];
        for (int j = 0; j < 8; ++j) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

bool ModbusRTU::readHoldingRegisters(uint16_t start_register, uint16_t register_count,
                                    std::vector<uint16_t>& out_values) {
    out_values.clear();

    uint8_t request[8]; // 地址(1)+功能码(1)+起始地址(2)+数量(2)+CRC(2)
    request[0] = static_cast<uint8_t>(slave_address_);
    request[1] = 0x03;
    request[2] = static_cast<uint8_t>((start_register >> 8) & 0xFF);
    request[3] = static_cast<uint8_t>(start_register & 0xFF);
    request[4] = static_cast<uint8_t>((register_count >> 8) & 0xFF);
    request[5] = static_cast<uint8_t>(register_count & 0xFF);

    uint16_t crc = calculateCRC(request, 6);
    request[6] = static_cast<uint8_t>(crc & 0xFF);
    request[7] = static_cast<uint8_t>((crc >> 8) & 0xFF);

    ssize_t written = write(serial_fd_, request, 8);
    if (written != 8) {
        std::cerr << "发送请求失败" << std::endl;
        return false;
    }

    const size_t max_response_length = 256;
    uint8_t response[max_response_length];
    fd_set read_fds;
    FD_ZERO(&read_fds);
    FD_SET(serial_fd_, &read_fds);

    struct timeval timeout = {1, 0};  // 1秒超时
    int select_result = select(serial_fd_ + 1, &read_fds, nullptr, nullptr, &timeout);
    if (select_result <= 0) {
        std::cerr << "等待响应超时" << std::endl;
        return false;
    }

    ssize_t bytes_read = read(serial_fd_, response, max_response_length);
    if (bytes_read < 5) {
        std::cerr << "响应数据长度不足" << std::endl;
        return false;
    }
 

    if (response[1] == 0x83) {
        std::cerr << "接收到异常响应" << std::endl;
        return false;
    }

    uint8_t byte_count = response[2];
    if (bytes_read < (3 + byte_count + 2)) {
        std::cerr << "响应数据不完整" << std::endl;
        return false;
    }

    // CRC 总是在响应帧的最后两个字节
    uint16_t received_crc = (static_cast<uint16_t>(response[bytes_read - 1]) << 8) | response[bytes_read - 2];
    /*
        printf("收到响应数据[%ld] byte_count=%d : \n",bytes_read,byte_count);
        for (ssize_t i = 0; i < bytes_read; ++i) {
            printf("%02X ", response[i]);
        }
    */
    printf("\n");
    uint16_t calculated_crc = calculateCRC(response, bytes_read - 2);
    if (received_crc != calculated_crc) {
        std::cerr << "CRC 校验失败" << std::endl;
        return false;
    }

    for (int i = 0; i < register_count; ++i) {
        uint16_t value = (static_cast<uint16_t>(response[3 + i * 2]) << 8) |
                         response[4 + i * 2];
        out_values.push_back(value);
    }

    return true;
}